#ifndef COOP_H
#define COOP_H
#include <vector>
#include <map>
// #include <Eigen>
#include <Eigen/Core>                    // 矩阵运算
#include <Eigen/QR>
#include <bits/stdc++.h>

namespace COOP
{

class COOPFunction
{
    public:

        COOPFunction(double _thhold_1,double _thhold_2);
        COOPFunction();
        double Virtual_Force_Real;
        double thhold_1;
        double thhold_2;
        double Repulsor_point;
        double Repulsor_point_max;
        Eigen::VectorXd RobotCommand;
        Eigen::VectorXd HistoryCommand;
        double size_k; // delta_speed
        double History_mean;
        double History_std;
        double k_h;
        double k_r;
        //Set Cooperative Function
        double VirtualForcePredictUpdate(double _Virtual_Force_Real);
        void Get_MPC_Command();
        void VirtualForceRealUpdate(double x);
        void UpdateHistoryCommand(double _historycommand);
        void SettingMatrix();
        void Intialize_parameter(double _thhold_1,double _thhold_2);
        // predicted value
};
}

#endif /* COOP_H*/